package kit.edu.lego.gruppe3.levels.line;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import lejos.nxt.LightSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;

public class LineFinder implements Behavior {

	public boolean suppressed = false;
	private DifferentialPilot robot = ComponentsRepository.getInstance()
			.getRobotPilot();
	LightSensor lightSensor = ComponentsRepository.getInstance()
			.getLightSensor();
	private int rotation = 1;

	@Override
	public void action() {

		rotation = 35;
		robot.setRotateSpeed(60);

		while (rotation < (LineSolver.getState() == 4 ? 70 : 210)) {
			robot.stop();
			if (ComponentsRepository.getLastTurn() == 'l') {
				robot.rotate(-1 * (rotation += 35), true);
				while (robot.isMoving())
					if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD) {
						robot.stop();
						return;
					}
				ComponentsRepository.setLastTurn('r');
			} else if (ComponentsRepository.getLastTurn() == 'r') {
				robot.rotate((rotation += 35), true);
				while (robot.isMoving())
					if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD) {
						robot.stop();
						return;
					}
				ComponentsRepository.setLastTurn('l');
			}
		}
		ComponentsRepository.endOfLine = true;
		if (ComponentsRepository.getLastTurn() == 'l')
			robot.rotate(-105);
		else
			robot.rotate(+105);
		robot.travel(1);
	}

	@Override
	public void suppress() {
		suppressed = true;
	}

	@Override
	public boolean takeControl() {
		return LevelSelector.getInstance().isLine()
				&& !ComponentsRepository.evadeObstacle
				// && !ComponentsRepository.endOfLine
				&& lightSensor.readValue() < ComponentsRepository.LIGHT_TRASHOLD;
	}

}
